#include "../globals/globals.h"
#include "../pid/pid.h"
#include "../_includes.h"
#include <iostream>
using namespace std;

#define Cchassis 2.75
#define wheelparam 79.7964534
#define Gear_Prop 1
#define wheelscale	1


double speed_start(double aimdist, double aimspeed, double acc, double timeout)
{
    chassis.reset_enc();

    double L_rot = 0, R_rot = 0;
    double L_rot_temp = mtr_LF.position(rotationUnits::deg), R_rot_temp = mtr_RF.position(rotationUnits::deg);

    vex::timer lin_timeout;
    lin_timeout.clear();
    double dist = 0;

    double distemp = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) / 2
     * Cchassis * wheelparam * Gear_Prop * wheelscale / 360;
    pid_ctrl pidspd = pid_ctrl::get_instance(0.8, 0.008, 0, 100, 0, 5, 10, 100);
    pid_ctrl pidyaw = pid_ctrl::get_instance(1.8, 0.008, 0, 4, 0, 5, 10, 100);

    double aim_speed_now = 0;

    while (lin_timeout.time() < timeout && abs(dist) < abs(aimdist))
    {

        L_rot = mtr_LF.position(rotationUnits::deg) - L_rot_temp;
        R_rot = mtr_RF.position(rotationUnits::deg) - R_rot_temp;

        dist = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) / 2 
        * Cchassis * wheelparam * Gear_Prop * wheelscale / 360 - distemp;
        // dist*=fwd;
        aim_speed_now += acc / 10;
        if (aim_speed_now > aimspeed)
            aim_speed_now = aimspeed;

        // speed_internal();
        chassis.FT_set_voltage((aim_speed_now + pidspd.pid_output_need_err
        (aimspeed - (chassis.get_actual_velocity(1) + chassis.get_actual_velocity(-1)) * sgn(aimdist) * 0.5)) * sgn(aimdist), 
        pidyaw.pid_output_need_err(R_rot - L_rot));
        // cout<< chassis.get_actual_velocity(-1) <<endl;

        this_thread::sleep_for(5);
    }
    // spd->chassisbrake(coast);
    // this_thread::sleep_for(100);
    return dist;
}

#define acc_len 0.75
#define slowdis 300

void stable_go(double aimdist, double aimspeed, double acc, double timeout)
{

    vex::timer fade_timeout;
    fade_timeout.clear();
    chassis.reset_enc();
    double L_rot = 0, R_rot = 0;
    double L_rot_temp = mtr_LF.position(rotationUnits::deg), R_rot_temp = mtr_RF.position(rotationUnits::deg);


    pid_ctrl pidend = pid_ctrl::get_instance(0.28, 0.003, 0.02, 0.6 * aimspeed, 0, 5, 10, 100);//1500 0.38 0.003
    pid_ctrl pidyaw = pid_ctrl::get_instance(1.8, 0.008, 0, 4, 0, 5, 10, 100);

    double dist_temp_end = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) 
    / 2 * Cchassis * wheelparam * Gear_Prop * wheelscale / 360;
    double dist_end = 0;

    if(abs(acc_len*aimdist) > slowdis)
    {
        speed_start(aimdist - slowdis*sgn(aimdist), aimspeed, acc, timeout);
    }
    else
    {
        speed_start(acc_len * aimdist, aimspeed, acc, timeout);
    }

    while (fade_timeout.time() < timeout && (abs(aimdist - dist_end) > 5 || ((abs(chassis.get_actual_velocity(1) + chassis.get_actual_velocity(-1)) * 0.5) > 5)))
    {
        L_rot = mtr_LF.position(rotationUnits::deg) - L_rot_temp;
        R_rot = mtr_RF.position(rotationUnits::deg) - R_rot_temp;

        double PIDinput = aimdist - dist_end;

        PIDinput *= 0.4 * (abs(aimdist - dist_end) / abs(aimdist) / 0.4 + 0.1);
        // 1500 0.5 0.4 0.1 400

        // cout << "in the loop" << aimdist-dist_end<< endl << chassis.get_actual_velocity(1)<<endl;
        // cout << chassis.get_actual_velocity(1)<<endl;
        dist_end = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) / 2 * Cchassis * wheelparam * Gear_Prop * wheelscale / 360 - dist_temp_end;

        chassis.FT_set_voltage(pidend.pid_output_need_err(PIDinput), pidyaw.pid_output_need_err(R_rot - L_rot));
        wait(1);
    }

    cout << "dist: " << dist_end<<endl;

    chassis.set_stopping(hold);
    chassis.stop();
    this_thread::sleep_for(50);
}

void fade_end(double aimdist, double aimspeed, double timeout)
{

    vex::timer fade_timeout;
    fade_timeout.clear();

    pid_ctrl pidend = pid_ctrl::get_instance(0.5, 0.005, 0, 0.6 * aimspeed, 0, 5, 10, 100);

    double dist_temp_end = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) / 2 * Cchassis * wheelparam * Gear_Prop * wheelscale / 360;
    double dist_end = 0;

    while (fade_timeout.time() < timeout && abs(aimdist - dist_end) > 10 || (abs(chassis.get_actual_velocity(-1) + chassis.get_actual_velocity(1)) * 0.5) > 1)
    {

        double PIDinput = aimdist - dist_end;

        PIDinput *= 0.9 * (abs(aimdist - dist_end) / aimdist / 2 + 0.1);

        cout << "in the loop" << endl;
        dist_end = (mtr_LF.position(rotationUnits::deg) + mtr_RF.position(rotationUnits::deg)) / 2 * Cchassis * wheelparam * Gear_Prop * wheelscale / 360 - dist_temp_end;

        chassis.FT_set_voltage(pidend.pid_output_need_err(PIDinput), 0);
    }

    cout << "dist:" << endl;

    chassis.set_stopping(hold);
    chassis.stop();
    this_thread::sleep_for(50);
}

